#ifndef __ASTAR_OPEN_LIST_HPP__
#define __ASTAR_OPEN_LIST_HPP__

#include <set>
#include <xutility>

namespace Astar
{
	//
	// AstarOpenList
	//
	template<typename key, typename compaer = std::less<key>, typename alloc = std::allocator<key>>
	class astar_open_list : public std::multiset<key, compaer, alloc>
	{
	protected:
		typedef key key_type;
		typedef key value_type;
		typedef compaer key_compare;
		typedef alloc key_allocator;
		typedef std::multiset<key_type, key_compare, key_allocator> base_type;
	public:
		typedef base_type::_Node node_type;
	public:
		astar_open_list()
			: base_type(key_compare(), key_allocator())
		{}
		~astar_open_list()
		{
			clear();
		}
	public:
		iterator insert(const value_type& _Val)
		{	// insert a key value
			return (xtree_insert(_Val).first);
		}
		iterator erase(const_iterator _Where)
		{	// erase element at _Where

#if _HAS_ITERATOR_DEBUGGING
			if (_Where._Mycont != this || _Isnil(_Where._Mynode()))
				std::_DEBUG_ERROR("map/set erase iterator outside range");
			_Nodeptr _Erasednode = _Where._Mynode();	// node to erase
			++_Where;	// save successor iterator for return
			_Orphan_ptr(*this, _Erasednode);

#else /* _HAS_ITERATOR_DEBUGGING */
			if (_Isnil(_Where._Mynode()))
			_THROW(std::out_of_range, "invalid map/set<T> iterator");
			_Nodeptr _Erasednode = _Where._Mynode();	// node to erase
			++_Where;	// save successor iterator for return
#endif /* _HAS_ITERATOR_DEBUGGING */

			_Nodeptr _Fixnode;	// the node to recolor as needed
			_Nodeptr _Fixnodeparent;	// parent of _Fixnode (which may be nil)
			_Nodeptr _Pnode = _Erasednode;

			if (_Isnil(_Left(_Pnode)))
				_Fixnode = _Right(_Pnode);	// must stitch up right subtree
			else if (_Isnil(_Right(_Pnode)))
				_Fixnode = _Left(_Pnode);	// must stitch up left subtree
			else
			{	// two subtrees, must lift successor node to replace erased
				_Pnode = _Where._Mynode();	// _Pnode is successor node
				_Fixnode = _Right(_Pnode);	// _Fixnode is its only subtree
			}

			if (_Pnode == _Erasednode)
			{	// at most one subtree, relink it
				_Fixnodeparent = _Parent(_Erasednode);
				if (!_Isnil(_Fixnode))
					_Parent(_Fixnode) = _Fixnodeparent;	// link up

				if (_Root() == _Erasednode)
					_Root() = _Fixnode;	// link down from root
				else if (_Left(_Fixnodeparent) == _Erasednode)
					_Left(_Fixnodeparent) = _Fixnode;	// link down to left
				else
					_Right(_Fixnodeparent) = _Fixnode;	// link down to right

				if (_Lmost() == _Erasednode)
					_Lmost() = _Isnil(_Fixnode)
					? _Fixnodeparent	// smallest is parent of erased node
					: _Min(_Fixnode);	// smallest in relinked subtree

				if (_Rmost() == _Erasednode)
					_Rmost() = _Isnil(_Fixnode)
					? _Fixnodeparent	// largest is parent of erased node
					: _Max(_Fixnode);	// largest in relinked subtree
			}
			else
			{	// erased has two subtrees, _Pnode is successor to erased
				_Parent(_Left(_Erasednode)) = _Pnode;	// link left up
				_Left(_Pnode) = _Left(_Erasednode);	// link successor down

				if (_Pnode == _Right(_Erasednode))
					_Fixnodeparent = _Pnode;	// successor is next to erased
				else
				{	// successor further down, link in place of erased
					_Fixnodeparent = _Parent(_Pnode);	// parent is successor's
					if (!_Isnil(_Fixnode))
						_Parent(_Fixnode) = _Fixnodeparent;	// link fix up
					_Left(_Fixnodeparent) = _Fixnode;	// link fix down
					_Right(_Pnode) = _Right(_Erasednode);	// link successor down
					_Parent(_Right(_Erasednode)) = _Pnode;	// link right up
				}

				if (_Root() == _Erasednode)
					_Root() = _Pnode;	// link down from root
				else if (_Left(_Parent(_Erasednode)) == _Erasednode)
					_Left(_Parent(_Erasednode)) = _Pnode;	// link down to left
				else
					_Right(_Parent(_Erasednode)) = _Pnode;	// link down to right

				_Parent(_Pnode) = _Parent(_Erasednode);	// link successor up
				_STD swap(_Color(_Pnode), _Color(_Erasednode));	// recolor it
			}

			if (_Color(_Erasednode) == _Black)
			{	// erasing black link, must recolor/rebalance tree
				for (; _Fixnode != _Root() && _Color(_Fixnode) == _Black;
					_Fixnodeparent = _Parent(_Fixnode))
					if (_Fixnode == _Left(_Fixnodeparent))
					{	// fixup left subtree
						_Pnode = _Right(_Fixnodeparent);
						if (_Color(_Pnode) == _Red)
						{	// rotate red up from right subtree
							_Color(_Pnode) = _Black;
							_Color(_Fixnodeparent) = _Red;
							_Lrotate(_Fixnodeparent);
							_Pnode = _Right(_Fixnodeparent);
						}

						if (_Isnil(_Pnode))
							_Fixnode = _Fixnodeparent;	// shouldn't happen
						else if (_Color(_Left(_Pnode)) == _Black
							&& _Color(_Right(_Pnode)) == _Black)
						{	// redden right subtree with black children
							_Color(_Pnode) = _Red;
							_Fixnode = _Fixnodeparent;
						}
						else
						{	// must rearrange right subtree
							if (_Color(_Right(_Pnode)) == _Black)
							{	// rotate red up from left sub-subtree
								_Color(_Left(_Pnode)) = _Black;
								_Color(_Pnode) = _Red;
								_Rrotate(_Pnode);
								_Pnode = _Right(_Fixnodeparent);
							}

							_Color(_Pnode) = _Color(_Fixnodeparent);
							_Color(_Fixnodeparent) = _Black;
							_Color(_Right(_Pnode)) = _Black;
							_Lrotate(_Fixnodeparent);
							break;	// tree now recolored/rebalanced
						}
					}
					else
					{	// fixup right subtree
						_Pnode = _Left(_Fixnodeparent);
						if (_Color(_Pnode) == _Red)
						{	// rotate red up from left subtree
							_Color(_Pnode) = _Black;
							_Color(_Fixnodeparent) = _Red;
							_Rrotate(_Fixnodeparent);
							_Pnode = _Left(_Fixnodeparent);
						}
						if (_Isnil(_Pnode))
							_Fixnode = _Fixnodeparent;	// shouldn't happen
						else if (_Color(_Right(_Pnode)) == _Black
							&& _Color(_Left(_Pnode)) == _Black)
						{	// redden left subtree with black children
							_Color(_Pnode) = _Red;
							_Fixnode = _Fixnodeparent;
						}
						else
						{	// must rearrange left subtree
							if (_Color(_Left(_Pnode)) == _Black)
							{	// rotate red up from right sub-subtree
								_Color(_Right(_Pnode)) = _Black;
								_Color(_Pnode) = _Red;
								_Lrotate(_Pnode);
								_Pnode = _Left(_Fixnodeparent);
							}

							_Color(_Pnode) = _Color(_Fixnodeparent);
							_Color(_Fixnodeparent) = _Black;
							_Color(_Left(_Pnode)) = _Black;
							_Rrotate(_Fixnodeparent);
							break;	// tree now recolored/rebalanced
						}
					}

					_Color(_Fixnode) = _Black;	// ensure stopping node is black
			}

			this->_Alnod.destroy(_Erasednode);	// destroy, free erased node

			if (0 < _Mysize)
				--_Mysize;

			return (_Make_iter(_Where));	// return successor iterator
		}
		size_type erase(const key_type& _Keyval)
		{	// erase and count all that match _Keyval
			_Pairii _Where = equal_range(_Keyval);
			size_type _Num = 0;
			_Distance(_Where.first, _Where.second, _Num);
			erase(_Where.first, _Where.second);
			return (_Num);
		}
		iterator erase(const_iterator _First, const_iterator _Last)
		{	// erase [_First, _Last)
			if (_First == begin() && _Last == end())
			{	// erase all
				clear();
				return (begin());
			}
			else
			{	// partial erase, one at a time
				while (_First != _Last)
					erase(_First++);
				return (_Make_iter(_First));
			}
		}
		void clear()
		{	// erase all
			using namespace std;

#if _HAS_ITERATOR_DEBUGGING
			this->_Orphan_ptr(*this, 0);
#endif /* _HAS_ITERATOR_DEBUGGING */

			_Erase(_Root());
			_Root() = _Myhead, _Mysize = 0;
			_Lmost() = _Myhead, _Rmost() = _Myhead;
		}
	private:
		_Pairib xtree_insert(const value_type& _Val)
		{	
			using namespace std;
			// try to insert node with value _Val
			_Nodeptr _Trynode = _Root();
			_Nodeptr _Wherenode = _Myhead;
			bool _Addleft = true;	// add to left of head if tree empty
			while (!_Isnil(_Trynode))
			{	// look for leaf to insert before (_Addleft) or after
				_Wherenode = _Trynode;
				_Addleft = _DEBUG_LT_PRED(this->comp,
					this->_Kfn(_Val), _Key(_Trynode));
				_Trynode = _Addleft ? _Left(_Trynode) : _Right(_Trynode);
			}

			if (this->_Multi)
				return (_Pairib(_Insert(_Addleft, _Wherenode, _Val), true));
			else
			{	// insert only if unique
				iterator _Where = _TREE_ITERATOR(_Wherenode);
				if (!_Addleft)
					;	// need to test if insert after is okay
				else if (_Where == begin())
					return (_Pairib(_Insert(true, _Wherenode, _Val), true));
				else
					--_Where;	// need to test if insert before is okay

				if (_DEBUG_LT_PRED(this->comp,
					_Key(_Where._Mynode()), this->_Kfn(_Val)))
					return (_Pairib(_Insert(_Addleft, _Wherenode, _Val), true));
				else
					return (_Pairib(_Where, false));
			}
		}
		iterator _Insert(bool _Addleft, _Nodeptr _Wherenode, const value_type& _Val)
		{	// add node with value next to _Wherenode, to left if _Addnode
			if (max_size() - 1 <= _Mysize)
				_THROW(std::length_error, "map/set<T> too long");
			_Nodeptr _Newnode = _Buynode(_Myhead, _Wherenode, _Myhead,
				_Val, _Red);

			++_Mysize;
			if (_Wherenode == _Myhead)
			{	// first node in tree, just set head values
				_Root() = _Newnode;
				_Lmost() = _Newnode, _Rmost() = _Newnode;
			}
			else if (_Addleft)
			{	// add to left of _Wherenode
				_Left(_Wherenode) = _Newnode;
				if (_Wherenode == _Lmost())
					_Lmost() = _Newnode;
			}
			else
			{	// add to right of _Wherenode
				_Right(_Wherenode) = _Newnode;
				if (_Wherenode == _Rmost())
					_Rmost() = _Newnode;
			}

			for (_Nodeptr _Pnode = _Newnode; _Color(_Parent(_Pnode)) == _Red; )
				if (_Parent(_Pnode) == _Left(_Parent(_Parent(_Pnode))))
				{	// fixup red-red in left subtree
					_Wherenode = _Right(_Parent(_Parent(_Pnode)));
					if (_Color(_Wherenode) == _Red)
					{	// parent has two red children, blacken both
						_Color(_Parent(_Pnode)) = _Black;
						_Color(_Wherenode) = _Black;
						_Color(_Parent(_Parent(_Pnode))) = _Red;
						_Pnode = _Parent(_Parent(_Pnode));
					}
					else
					{	// parent has red and black children
						if (_Pnode == _Right(_Parent(_Pnode)))
						{	// rotate right child to left
							_Pnode = _Parent(_Pnode);
							_Lrotate(_Pnode);
						}
						_Color(_Parent(_Pnode)) = _Black;	// propagate red up
						_Color(_Parent(_Parent(_Pnode))) = _Red;
						_Rrotate(_Parent(_Parent(_Pnode)));
					}
				}
				else
				{	// fixup red-red in right subtree
					_Wherenode = _Left(_Parent(_Parent(_Pnode)));
					if (_Color(_Wherenode) == _Red)
					{	// parent has two red children, blacken both
						_Color(_Parent(_Pnode)) = _Black;
						_Color(_Wherenode) = _Black;
						_Color(_Parent(_Parent(_Pnode))) = _Red;
						_Pnode = _Parent(_Parent(_Pnode));
					}
					else
					{	// parent has red and black children
						if (_Pnode == _Left(_Parent(_Pnode)))
						{	// rotate left child to right
							_Pnode = _Parent(_Pnode);
							_Rrotate(_Pnode);
						}
						_Color(_Parent(_Pnode)) = _Black;	// propagate red up
						_Color(_Parent(_Parent(_Pnode))) = _Red;
						_Lrotate(_Parent(_Parent(_Pnode)));
					}
				}

				_Color(_Root()) = _Black;	// root is always black
				return (_TREE_ITERATOR(_Newnode));
		}

		_Nodeptr _Buynode(_Nodeptr _Larg, _Nodeptr _Parg,
			_Nodeptr _Rarg, const value_type& _Val, char _Carg)
		{	// allocate a node with pointers, value, and color
			_Nodeptr _Wherenode = (_Nodeptr)_Val;
			_TRY_BEGIN
				new (_Wherenode) _Node(_Larg, _Parg, _Rarg, _Val, _Carg);
			_CATCH_ALL

				_RERAISE;
			_CATCH_END
				return (_Wherenode);
		}
		void _Erase(_Nodeptr _Rootnode)
		{	// free entire subtree, recursively
			for (_Nodeptr _Pnode = _Rootnode; !_Isnil(_Pnode); _Rootnode = _Pnode)
			{	// free subtrees, then node
				_Erase(_Right(_Pnode));
				_Pnode = _Left(_Pnode);
				this->_Alnod.destroy(_Rootnode);	// destroy, free erased node
			}
		}
	};
}// namespace Astar

#endif//__ASTAR_OPEN_LIST_HPP__